#include <hdd.h>
#include <isr.h>
#include <i386.h>
#include <sem.h>
#include <mm.h>
#include <string.h>

#define MASTER 0x40
#define SLAVE  0x50

#define MAX_PORTS 2
#define MAX_DISKS 2*MAX_PORTS
#define HDD_BSY  0x80
#define HDD_DRDY 0x40
#define HDD_DRQ  0x08
#define HDD_ERR  0x01
#define SECTORS_PER_COMMAND (4096 / 512)

extern void hdd_isr_14();
extern void hdd_isr_15();

typedef struct str_hdd_blockdev{
	blockdev bd;
	uint_32 disk;
} __attribute__((packed)) hdd_blockdev;

typedef struct str_ata_port{
	uint_16 base;
	uint_16 asr;
	sem_t sem;
	uint_32 last_read;
	uint_16 *tmp;
} ata_port;

typedef struct str_hdd{
	uint_8  present;
	uint_8  master;
	uint_64 sector_count;
	ata_port *port;
	sem_t sem;
	uint_32 pos;
	uint_32 sectors_left;
} hdd;

ata_port ports[MAX_PORTS];

hdd disks[MAX_PORTS * 2];

static inline void wait400(ata_port *port){
	int i;
	for(i = 0; i < 4; ++i) inb(port->asr);
}

void hdd_isr_c(uint_32 pindex){
	//breakpoint();
	ata_port *pstr = ports + pindex;
	hdd *disk = disks + pstr->last_read;
	uint_8 status = inb(pstr->base + 7);
	if(status & HDD_ERR){
		//An error ocurred. For now, wake the thread right now
		//TODO: Implement proper error handling
		sem_signaln(&disk->sem);
		return;
	}
	//~ breakpoint();
	insw(pstr->base, pstr->tmp + disk->pos, 256);
	disk->pos += 256;
	wait400(pstr);
	if(!(--disk->sectors_left)){
		//breakpoint();
		sem_signaln(&disk->sem);
	}
}

sint_32 hdd_block_write(blockdev* this, uint_32 pos, const void* buf, uint_32 size) {
	return -1; //Opcional
}

uint_32 hdd_block_flush(blockdev *this){
	mm_mem_free(this);
	return 0;
}

static inline void drive_select(uint_32 disk){
	uint_8 port = disks[disk].port->base;
	outb(port + 6, disks[disk].master);
	if(disks[disk].port->last_read == disk) return;
	disks[disk].port->last_read = disk;
	//"Wait" 400ns, per http://wiki.osdev.org/ATA_PIO_Mode#400ns_delays.
	wait400(disks[disk].port);
}

sint_32 hdd_block_read(blockdev* this, uint_32 pos, void* buf, uint_32 size) {
	uint_16 *dest = buf;
	uint_64 rpos = pos;
	uint_32 write_from = 0;
	hdd_blockdev *hddev = (hdd_blockdev *) this;
	sint_32 status = 0;
	hdd *str = disks + hddev->disk;
	uint_16 port = str->port->base;
	while(size > 0){
		uint_32 rsize = size % SECTORS_PER_COMMAND;
		if(!rsize) rsize = SECTORS_PER_COMMAND;
		//Wait for bus
		sem_wait(&str->port->sem);
		//Make sure sem_wait is executed BEFORE the first block arrives
		cli();
		drive_select(hddev->disk);
		//Setup disk structure
		str->pos = 0;
		str->sectors_left = rsize;
		//breakpoint();
		while((inb(str->port->asr) & HDD_BSY)) //breakpoint();
		if(status & HDD_ERR) {
			sem_signaln(&str->port->sem);
			return 1;
		}
		//Enable interrupt mode
		outb(str->port->asr, 0);
		//Send LBA Dir
		outb(port + 2, rsize >> 8);
		outb(port + 3, rpos >> 24);
		outb(port + 4, rpos >> 32);
		outb(port + 5, rpos >> 48);
		outb(port + 2, rsize);
		outb(port + 3, rpos >> 0);
		outb(port + 4, rpos >> 8);
		outb(port + 5, rpos >> 16);
		//Send command
		outb(port + 7, 0x24);
		//Wait for command completition
		sem_wait(&str->sem);
		if(str->sectors_left){
			//TODO: Determine error
			status = 1;
			sem_signaln(&str->port->sem);
			return status;
		}
		memcpy(dest + write_from, str->port->tmp, str->pos * sizeof(uint_16));
		write_from += str->pos;
		size -= rsize;
		rpos += rsize;
		sti();
		sem_signaln(&str->port->sem);
	}
	return status;
}

static inline uint_64 hdd_test(ata_port *pstr, uint_8 drive){
	uint_16 status;
	uint_16 identify[256];
	uint_64 sectors;
	uint_16 port = pstr->base;
	uint_16  asr  = pstr->asr;
	outb(port + 6, drive);
	wait400(pstr);
	outb(port + 7, 0xEC);
	wait400(pstr);
	status = inb(asr);
	if(!status) return 0;
	while(status & HDD_BSY){
	       status = inb(asr);
	}
	if (inb(port + 4) != 0 || inb(port + 5) != 0){
		outb(asr, 0);
		return 0;
	}

	while(!(status & (HDD_ERR | HDD_DRQ))){
		status = inb(asr);
	}
	if(status & HDD_ERR){
		outb(asr, 0);
		return 0;
	}
	status = inb(port + 7);
	insw(port, identify, 256);
	//Accept only drives that support LBA48
	wait400(pstr);
	inb(pstr->asr);
	if (!(identify[83] & 1024)){
		outb(pstr->asr, 0);
		return 0;
	}
	sectors = 0;
	memcpy(&sectors, identify + 100, 8);
	outb(pstr->asr, 0);
	outb(0x20, 0x20);
	outb(0xA0, 0x20);
	return sectors;
}

blockdev* hdd_open(int nro) {
	hdd_blockdev *this;
	blockdev *res = 0;
	if(!disks[nro].port) return 0;
	this = mm_mem_kalloc();
	if(!this) return res;
	res = (blockdev *) this;
	this->bd.clase    = BLOCKDEV;
	this->bd.refcount = 1;
	this->bd.flush = hdd_block_flush;
	this->bd.write = hdd_block_write;
	this->bd.read = hdd_block_read;
	this->disk = nro;
	return res;
}

void init_port(ata_port *str, uint_16 port, uint_16 asr){
	outb(asr, 2);
	str->base = port;
	str->asr  = asr;
	str->sem = SEM_NEW(1);
	str->last_read = -1;
	str->tmp = NULL;
}

void invalidate_disk(hdd *disk){
	disk->present = 0;
	disk->port = 0;
	disk->master = 0;
	disk->sectors_left = 0;
}

void valid_disk(hdd *disk, uint_32 sectors, ata_port *port, uint_16 pos){
	disk->present = 1;
	disk->port = port;
	disk->sector_count = sectors;
	disk->master = pos;
	disk->sem = SEM_NEW(0);
	disk->sectors_left = 0;
}

void hdd_init(void) {
	/* Standard IDE/ATA I/O Ports */
	uint_32 i;
	uint_8 status;
	init_port(ports, 0x1f0, 0x3f6);
	init_port(ports + 1, 0x170, 0x376);
	//Detect hard disks
	for(i = 0; i < MAX_PORTS; ++i){
		invalidate_disk(disks + 2 * i);
		invalidate_disk(disks + 2 * i + 1);
		uint_64 sectors = 0;
		uint_16 port = ports[i].base;
		status = inb(port + 7);
		if (status == 0xFF){
			continue;
		}
		sectors = hdd_test(ports + i, MASTER);
		if (sectors){
			if((ports[i].tmp = mm_mem_kalloc())){
				valid_disk(disks + (2 * i), sectors, ports + i, MASTER);
			}
		}
		//sectors = hdd_test(ports + i, SLAVE);
		sectors = 0;
		if (sectors){
			if(ports[i].tmp){
				valid_disk(disks + (2 * i) + 1, sectors, ports + i, SLAVE);
			}
		}
		//outb(ports[i].asr, 4);
	}
	for(i = 0; i < MAX_DISKS; ++i){
		if(disks[i].present){
			outb(disks[i].port->asr, 0);
		}
	}
	#if 0
	drive_select(0);
	while(1){
		inb(disks[0].port->asr);
		breakpoint();
	}
	#endif /*if 0*/
	idt_register(ISR_IRQ14, hdd_isr_14, 0);
	idt_register(ISR_IRQ15, hdd_isr_15, 0);
}
